#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include "TCLStereoDepthAlg.h"
#include "fbs.hpp"
#include "jbf.h"
#include "epf.h"
#include "native_debug.h"
#define CV_CPU_HAS_SUPPORT_NEON 1
#include "opencv2/core/hal/intrin.hpp"
#include <opencv2/opencv.hpp>
#include "colorCorrection.h"

#include "segment.h"

tcl_depth_image::TCLStereoDepthAlg_cap::TCLStereoDepthAlg_cap() {}

tcl_depth_image::TCLStereoDepthAlg_cap::~TCLStereoDepthAlg_cap() {}


void tcl_depth_image::TCLStereoDepthAlg_cap::Init(const Param_DepthCapture & param) {
	IMAGE_DEPTH_LOGI("INIT BEGIN");
	double tick_freq = cv::getTickFrequency();
	double tick_start = TICK(), tick_stop;
	double tick_sub_start = tick_start, tick_sub_stop;

    this->m_param.copy(param);
	this->target_size = cv::Size(816, 612);
	this->micro_dist_thr = 35;
	this->macro_dist_thr = 3; //3
	this->trigger_max_disp = 12;
	this->m_param.max_disp = 16;//32
	this->micro_dist_ratio = 0.75;

	if (m_param.alg_type == TCL_IMAGE_DEPTH_ALG_FAST) {
		this->m_param.depth_size = cv::Size(480, 360);
	}
	else {
		this->m_param.depth_size = cv::Size(640, 480);
	}

	if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90 ||
		m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		m_param.full_size = cv::Size(m_param.full_size.height, m_param.full_size.width);
		m_param.crop_size = cv::Size(m_param.crop_size.height, m_param.crop_size.width);
		m_param.depth_size = cv::Size(m_param.depth_size.height, m_param.depth_size.width);
	}

	if (m_param.alg_type == TCL_IMAGE_DEPTH_ALG_FAST) {
		this->margin_y = 24;
		this->margin_x = 36 + m_param.max_disp;
	}
	else {
		this->margin_y = 32;
		this->margin_x = 48 + m_param.max_disp;
	}

	this->min_disp = m_param.min_disp - m_param.delta_disp_per_degree;
	IMAGE_DEPTH_LOGI("\t calibrated min_disp=%6.3f", this->min_disp);
	this->mat_row_align.create(3, 3, CV_32FC1);

	if (m_param.alg_type == TCL_IMAGE_DEPTH_ALG_FAST) this->m_param.win_size = 7;
	else this->m_param.win_size = 9;

	this->m_param.num_feat = 2000;
	this->m_param.max_iter = 300;
	this->m_param.res_level = 0.5;
	this->m_param.num_worst = 3;

    // in case of cropping view on main camera
	int dpth_w = m_param.depth_size.width;
	int dpth_h = m_param.depth_size.height;
	int cali_w = std::fmax(m_param.cali_size.width, 1);
	int cali_h = std::fmax(m_param.cali_size.height, 1);
	int full_w = std::fmax(m_param.full_size.width, 1);
	int full_h = std::fmax(m_param.full_size.height, 1);
	int crop_w = std::fmax(m_param.crop_size.width, 1);
	int crop_h = std::fmax(m_param.crop_size.height, 1);

    crop_cali_size.width = cali_w - (cali_w / (float)full_w) * (full_w - crop_w);
    crop_cali_size.height = cali_h - (cali_h / (float)full_h) * (full_h - crop_h);

    down_scale = m_param.depth_size.width / (double)crop_cali_size.width;

	double fx1, fx2, fy1, fy2;
	fx1 = m_param.intrinsics.M1.at<double>(0, 0);
	fy1 = m_param.intrinsics.M1.at<double>(1, 1);
	fx2 = m_param.intrinsics.M2.at<double>(0, 0);
	fy2 = m_param.intrinsics.M2.at<double>(1, 1);
	scale_aux_x = down_scale * fx2 / fx1;
	scale_aux_y = down_scale * fy2 / fy1;

    crop_delta_size.width = down_scale * cali_w - dpth_w;
    crop_delta_size.height = down_scale * cali_h - dpth_h;

	// step 1. scale down main to depth size,
	// and scale down aux to the same zoom level of main in depth size.
    // update intrinsics for aux camera
    m_param.intrinsics.M1.at<double>(0, 0) *= scale_aux_x;
    m_param.intrinsics.M1.at<double>(0, 2) *= scale_aux_x;
    m_param.intrinsics.M1.at<double>(1, 1) *= scale_aux_y;
    m_param.intrinsics.M1.at<double>(1, 2) *= scale_aux_y;

	// update intrinsics for main camera
    m_param.intrinsics.M2.at<double>(0, 0) *= down_scale;
    m_param.intrinsics.M2.at<double>(0, 2) = m_param.intrinsics.M2.at<double>(0, 2) * down_scale - crop_delta_size.width / 2.0;
    m_param.intrinsics.M2.at<double>(1, 1) *= down_scale;
    m_param.intrinsics.M2.at<double>(1, 2) = m_param.intrinsics.M2.at<double>(1, 2) * down_scale - crop_delta_size.height / 2.0;

    // generate distortion maps
    cv::Mat E = cv::Mat::diag(cv::Mat::ones(cv::Size(1, 3), CV_64F));
    cv::Mat D = cv::Mat::zeros(cv::Size(8, 1), CV_64F);

    // normalize both views
	cv::Size aux_size;
	aux_size.width = scale_aux_x * cali_w;
	aux_size.height = scale_aux_y * cali_h;
	try {
		cv::initUndistortRectifyMap(
			m_param.intrinsics.M1,
			m_param.intrinsics.D1,
			E,
			m_param.intrinsics.M1,
			aux_size,
			CV_32F,
			map11, map12);
		cv::initUndistortRectifyMap(
			m_param.intrinsics.M2,
			m_param.intrinsics.D2,
			E,
			m_param.intrinsics.M2,
			m_param.depth_size,
			CV_32F,
			map21, map22);
	}
	catch (...) {
		init_success = false;
		IMAGE_DEPTH_LOGE("Init Failed: illegal calibration data!");
		return;
	}

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t distortion map generation: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	if (m_param.alg_type != TCL_IMAGE_DEPTH_ALG_FAST) {
		// inverse map for the right view only
		imap1 = Mat::zeros(m_param.depth_size, CV_32FC1);
		imap2 = Mat::zeros(m_param.depth_size, CV_32FC1);

		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t inverse map allocation: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		std::vector<cv::Point2f> pts, pts_dst;

		pts.resize(dpth_w * dpth_h);
		pts_dst.resize(dpth_w * dpth_h);

		for (int i = 0; i < dpth_h; i++) {
			for (int j = 0; j < dpth_w; j++) {
				pts[i * dpth_w + j].x = j;
				pts[i * dpth_w + j].y = i;
			}
		}

		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t points generation: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

#define OMP_THREADS 4
#pragma omp parallel for num_threads(OMP_THREADS)
		for (int idx = 0; idx < OMP_THREADS; idx++) {
			std::vector<cv::Point2f> pts_temp(
				pts.begin() +
				idx * dpth_w * dpth_h / OMP_THREADS,
				pts.begin() +
				(idx + 1) * dpth_w * dpth_h / OMP_THREADS);
			std::vector<cv::Point2f> pts_temp2(
				pts_dst.begin() +
				idx * dpth_w * dpth_h / OMP_THREADS,
				pts_dst.begin() +
				(idx + 1) * dpth_w * dpth_h / OMP_THREADS);
			cv::undistortPoints(
				pts_temp,
				pts_temp2,
				m_param.intrinsics.M2,
				m_param.intrinsics.D2,
				E,
				m_param.intrinsics.M2);
			std::copy(
				pts_temp2.begin(),
				pts_temp2.end(),
				pts_dst.begin() +
				idx * dpth_w * dpth_h / OMP_THREADS);
		}

		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t points undistortion: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		for (int i = 0; i < dpth_h; i++) {
			for (int j = 0; j < dpth_w; j++) {
				imap1.at<float>(i, j) = pts_dst[i * dpth_w + j].x;
				imap2.at<float>(i, j) = pts_dst[i * dpth_w + j].y;
			}
		}

		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t inverse map filling: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		pts.erase(pts.begin(), pts.end());
		pts_dst.erase(pts_dst.begin(), pts_dst.end());

		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t finalizing: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;
	}
	init_success = true;
	tick_stop = TICK();
	IMAGE_DEPTH_LOGI("INIT DONE: %.1f ms", (1000*(tick_stop - tick_start)/tick_freq));
}


void tcl_depth_image::TCLStereoDepthAlg_cap::Preprocess(
        const cv::Mat & im_L_o,
        const cv::Mat & im_R_o
	) {
	IMAGE_DEPTH_LOGI("PREPROCESS BEGIN");
    if(im_L_o.size().width <= 0){
		IMAGE_DEPTH_LOGE("INPUT IMAGES ARE EMPTY!");
		exit(-1);
    }

	double tick_freq = cv::getTickFrequency();
	double tick_start = TICK(), tick_stop;
	double tick_sub_start = tick_start, tick_sub_stop;

    // in case of cropping view on main camera
    int cali_w = m_param.cali_size.width;
    int cali_h = m_param.cali_size.height;
    int full_w = m_param.full_size.width;
    int full_h = m_param.full_size.height;
    int crop_w = m_param.crop_size.width;
    int crop_h = m_param.crop_size.height;
	int dpth_w = m_param.depth_size.width;
	int dpth_h = m_param.depth_size.height;

    // for main camera
	cv::resize(im_R_o, im_guide, target_size, cv::INTER_LINEAR);
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		cv::resize(im_guide, im_R, m_param.depth_size, 0, 0, INTER_LINEAR);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		cv::resize(im_guide, im_R, m_param.depth_size, 0, 0, INTER_LINEAR);
		cv::rotate(im_R, im_R, cv::ROTATE_180);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::Size tmp_size(m_param.depth_size.height, m_param.depth_size.width);
		cv::resize(im_guide, im_R, tmp_size, 0, 0, INTER_LINEAR);
		cv::rotate(im_R, im_R, cv::ROTATE_90_COUNTERCLOCKWISE);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::Size tmp_size(m_param.depth_size.height, m_param.depth_size.width);
		cv::resize(im_guide, im_R, tmp_size, 0, 0, INTER_LINEAR);
		cv::rotate(im_R, im_R, cv::ROTATE_90_CLOCKWISE);
	}

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t MAIN resize and rotate: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::remap(im_R, im_R_small, map21, map22, cv::INTER_LINEAR);
	im_R = cv::Mat::zeros(cv::Size(dpth_w + 2 * margin_x, dpth_h + 2 * margin_y), CV_8UC3);
	im_R_small.copyTo(im_R(cv::Rect(margin_x, margin_y, dpth_w, dpth_h)));

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t MAIN remap and pad: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

    // for aux camera
    cv::Size size_small = cv::Size((int)(scale_aux_x * cali_w), (int)(scale_aux_y * cali_h));
    cv::resize(im_L_o, im_L, size_small);
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		cv::resize(im_L_o, im_L, size_small);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		cv::resize(im_L_o, im_L, size_small, 0, 0, INTER_LINEAR);
		cv::rotate(im_L, im_L, cv::ROTATE_180);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::Size tmp_size(size_small.height, size_small.width);
		cv::resize(im_L_o, im_L, tmp_size);
		cv::rotate(im_L, im_L, cv::ROTATE_90_COUNTERCLOCKWISE);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::Size tmp_size(size_small.height, size_small.width);
		cv::resize(im_L_o, im_L, tmp_size);
		cv::rotate(im_L, im_L, cv::ROTATE_90_CLOCKWISE);
	}

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t AUX resize and rotate: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::remap(im_L, im_L_small, map11, map12, cv::INTER_LINEAR);
	float crop_x, crop_y;
	crop_x = (size_small.width - (dpth_w + 2 * margin_x)) / 2.0;
	crop_y = (size_small.height - (dpth_h + 2 * margin_y)) / 2.0;
	if (crop_x < 0 || crop_y < 0) {
		IMAGE_DEPTH_LOGE("\t cropping exceeding range of aux camera!");
		IMAGE_DEPTH_LOGE("\t crop_x: %6.1f crop_y: %6.1f", crop_x, crop_y);
		cv::Size tmp_size(dpth_w + 2 * margin_x, dpth_h + 2 * margin_y);
		im_L = cv::Mat::zeros(tmp_size, CV_8UC1);
		cv::Rect2f src_roi, dst_roi;
		src_roi.x = std::fmax(0, crop_x);
		src_roi.y = std::fmax(0, crop_y);
		src_roi.width = im_L_small.size().width - 2 * src_roi.x;
		src_roi.height = im_L_small.size().height - 2 * src_roi.y;
		dst_roi.x = std::fmax(0, -crop_x);
		dst_roi.y = std::fmax(0, -crop_y);
		dst_roi.width = src_roi.width;
		dst_roi.height = src_roi.height;
		im_L_small(src_roi).copyTo(im_L(dst_roi));
	}
	else {
		im_L = im_L_small(cv::Rect(
			crop_x,
			crop_y,
			dpth_w + 2 * margin_x,
			dpth_h + 2 * margin_y)).clone();
	}

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t AUX remap and crop/pad: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	ShowImg("remap_L", im_L);
	ShowImg("remap_R", im_R);

	real_max_disp = m_param.max_disp;
	min_disp = m_param.min_disp - m_param.delta_disp_per_degree;

    if (!RowAlign_grid(
		im_L_small,
		im_R, // main
		im_L, // aux
        m_param.num_feat,
        m_param.max_iter,
        m_param.res_level,
        m_param.num_worst,
		margin_x,
		margin_y,
		real_max_disp,
		min_disp,
		this->mat_row_align)
    ) {
		IMAGE_DEPTH_LOGE("!!! ROW-ALIGNMENT FAILED !!!");
    }

	im_R_small = im_R(cv::Rect(margin_x, margin_y, m_param.max_disp + dpth_w, dpth_h)).clone();
	im_L_small = im_L_small(cv::Rect(margin_x, margin_y, m_param.max_disp + dpth_w, dpth_h)).clone();

	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t row align and crop: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	if (m_param.dump_enabled) DumpImage(im_R_small, m_param.dump_path, "rowalign_r", uid);
	if (m_param.dump_enabled) DumpImage(im_L_small, m_param.dump_path, "rowalign_l", uid);

	tick_stop = TICK();
	IMAGE_DEPTH_LOGI("PREPROCESS DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
}

int tcl_depth_image::TCLStereoDepthAlg_cap::ComputeDepthImpl(
        cv::Mat & output,
        const cv::Mat & main_,
        const cv::Mat & aux_
){
	IMAGE_DEPTH_LOGI("\t ComputeDepthImpl BEGIN");

#pragma region initialization
	double tick_freq = cv::getTickFrequency();
	double tick_start = TICK(), tick_stop;
	double tick_sub_start = tick_start, tick_sub_stop;

	if (output.type() != CV_8UC1 || output.size() != main_.size()) output.create(main_.size(), CV_8UC1);
	cv::Mat main_grey, aux_grey;
#pragma endregion

#pragma region calculate min disparity
	// check the min-disparity according to bm in low-reso
	cv::Size low_reso(main_.size().width / 4, main_.size().height / 4);
	int bm_max_disp = std::max(16, m_param.max_disp/2);
	int bm_block_size = 9;
	cv::Ptr<cv::StereoBM> bm = cv::StereoBM::create(bm_max_disp, bm_block_size);
	bm->setUniquenessRatio(30);
	bm->setSpeckleRange(800);
	bm->setSpeckleWindowSize(32);
	cv::Mat main_bm, disp_bm_s16, disp_bm_u8;
	cv::Mat main_bm_gray, aux_bm_gray;

	cv::resize(main_, main_bm, low_reso);
	cv::resize(aux_, aux_bm_gray, low_reso);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm resize: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::cvtColor(main_bm, main_bm_gray, cv::COLOR_BGR2GRAY);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm rgb2gray: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	bm->compute(main_bm_gray, aux_bm_gray, disp_bm_s16);
	disp_bm_s16.convertTo(disp_bm_u8, CV_8UC1, 255.0 / (bm_max_disp * 16));
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm compute: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	if (m_param.dump_enabled) DumpImage(disp_bm_u8, m_param.dump_path, "disp_bm_u8", uid);

	// remove white spots
	float max_disp_bm = real_max_disp / 4;
	max_disp_bm = std::fmin(max_disp_bm * 1.2 * (256.0 / bm_max_disp) - 1, 255.0);
	cv::Mat mask_ = disp_bm_u8 <= (uint8_t)(max_disp_bm);
	disp_bm_u8 = disp_bm_u8.mul(mask_ / 255);
	ShowImg("bm-raw", disp_bm_u8);
	if (m_param.dump_enabled) DumpImage(disp_bm_u8, m_param.dump_path, "disp_bm_raw", uid);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm max clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	HistBasedFilter(disp_bm_u8, 0.01, true); // 0.04
	if (m_param.dump_enabled) DumpImage(disp_bm_u8, m_param.dump_path, "disp_bm_hist", uid);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm min clip(zero ignored): %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	FillBlackHoles_new(disp_bm_u8, main_bm, false);
	ShowImg("bm-FillBlackHoles_new", disp_bm_u8);
	if (m_param.dump_enabled) DumpImage(disp_bm_u8, m_param.dump_path, "disp_bm_fill_holes", uid);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm fill holes: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	HistBasedFilter(disp_bm_u8, 0.004, false);
	ShowImg("bm-HistBasedFilter", disp_bm_u8);
	if (m_param.dump_enabled) DumpImage(disp_bm_u8, m_param.dump_path, "disp_bm_hist_2", uid);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm min clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_bm =
		cv::ximgproc::createDisparityWLSFilterGeneric(false);
	wls_bm->setSigmaColor(0.4);//0.4
	wls_bm->setLambda(8000);//8000
	cv::Mat disp_bm_wls;
	wls_bm->filter(disp_bm_u8, disp_bm_u8, disp_bm_wls);
	ShowImg("bm-wls", disp_bm_wls);
	if (m_param.dump_enabled) DumpImage(disp_bm_u8, m_param.dump_path, "disp_bm_wls", uid);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm wls: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	float bm_min, bm_max;
	cv::Mat t_;
	cv::reduce(disp_bm_wls, t_, 0, cv::REDUCE_MIN);
	cv::reduce(t_, t_, 1, cv::REDUCE_MIN);
	bm_min = (float)(t_.at<uint8_t>(0, 0)) / 255.0 * bm_max_disp * 4 - 4; // -4
	IMAGE_DEPTH_LOGI("\t\t bm->min disparity= %6.3f", bm_min);

	bm_min = std::fmax(bm_min, 0);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t bm calc min disp: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;
#pragma endregion

#pragma region shift aux view
	// shift the aux view to enforce min disp to be 0
	cv::Mat aux_shifted = cv::Mat::zeros(aux_.size(), aux_.type());
	cv::Rect roi_dst(bm_min, 0, aux_.size().width - bm_min, aux_.size().height);
	cv::Rect roi_src(0, 0, aux_.size().width - bm_min, aux_.size().height);
	aux_(roi_src).copyTo(aux_shifted(roi_dst));
	real_max_disp -= bm_min;
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t shift aux: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;
#pragma endregion

#pragma region sgbm
	cv::Ptr<cv::StereoMatcher> left_matcher;
	if (m_param.alg_type == TCL_IMAGE_DEPTH_ALG_FAST) {
		cv::Ptr<cv::StereoSGBM> sgbm_ = cv::StereoSGBM::create();
		sgbm_->setBlockSize(m_param.win_size);
		sgbm_->setNumDisparities(m_param.max_disp);
		sgbm_->setMinDisparity(0);
		sgbm_->setPreFilterCap(63);//42
		sgbm_->setP1(24 * m_param.win_size * m_param.win_size);//24
		sgbm_->setP2(96 * m_param.win_size * m_param.win_size);//96
		sgbm_->setUniquenessRatio(30);//30
		sgbm_->setMode(cv::StereoSGBM::MODE_HH4); //MODE_HH4
		left_matcher = sgbm_;
	}
	else {
		/*cv::Ptr<cv::StereoBM> bm_ = cv::StereoBM::create(m_param.max_disp, 15);
		bm_->setUniquenessRatio(30);
		left_matcher = bm_;*/

		cv::Ptr<cv::StereoSGBM> sgbm_ = cv::StereoSGBM::create();
		sgbm_->setBlockSize(m_param.win_size);
		sgbm_->setNumDisparities(m_param.max_disp);
		sgbm_->setMinDisparity(0);
		sgbm_->setPreFilterCap(63);//42
		sgbm_->setP1(24 * m_param.win_size * m_param.win_size);//24
		sgbm_->setP2(96 * m_param.win_size * m_param.win_size);//96
		sgbm_->setUniquenessRatio(30);//30
		sgbm_->setMode(cv::StereoSGBM::MODE_HH4); //MODE_HH4
		left_matcher = sgbm_;
	}

	// calculate the size used for micro distance
	cv::Size micro_dist_size;
	micro_dist_size.width = this->micro_dist_ratio * (m_param.max_disp + m_param.depth_size.width);
	micro_dist_size.height = this->micro_dist_ratio * m_param.depth_size.height;

	// calculate disparity in low resolution in micro-distance shot mode
	cv::Mat main_resized;
	if (real_max_disp > micro_dist_thr) {
		IMAGE_DEPTH_LOGI("\t\t !!! Micro-Distance Shot detected !!!");
		// directly return with full white depth map to avoid breaking optical bokeh
		output = cv::Mat::zeros(target_size, CV_8UC1) + 255;
		return 1;
	} else if (real_max_disp > trigger_max_disp) {
		IMAGE_DEPTH_LOGI("\t\t !!! Using Low Resolution Depth Map !!!");
		cv::resize(main_, main_resized, micro_dist_size);
		cv::resize(aux_shifted, aux_grey, micro_dist_size);
	} else if (real_max_disp <= macro_dist_thr) {
		IMAGE_DEPTH_LOGI("\t\t !!! Macro-Distance Shot detected !!!");
		// directly return with full white depth map to avoid mis-bokeh effect
		output = cv::Mat::zeros(target_size, CV_8UC1) + 255;
		return 1;
	} else {
		main_resized = main_;
		aux_grey = aux_shifted;
	}

	cv::cvtColor(main_resized, main_grey, cv::COLOR_BGR2GRAY);
	ShowImg("main_grey", main_grey);
	ShowImg("aux_grey", aux_grey);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t resize/- and rgb2gray: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;


//#define EXPORT_FOR_RGBD_PORTRAIT
#ifdef EXPORT_FOR_RGBD_PORTRAIT
	char buf[16];
	std::memset(buf, 0, sizeof(buf));
	sprintf_s(buf, "%02dmain", (int)(1.2*real_max_disp));
	DumpImage(main_, m_param.dump_path, cv::String(buf), uid);

	std::memset(buf, 0, sizeof(buf));
	sprintf_s(buf, "%02daux", (int)(1.2*real_max_disp));
	DumpImage(aux_grey, m_param.dump_path, cv::String(buf), uid);
#endif

	cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(6.0, cv::Size(11,11));
	clahe->apply(aux_grey, aux_grey);
	clahe->apply(main_grey, main_grey);
	ShowImg("main-clahe", main_grey);
	ShowImg("aux-clahe", aux_grey);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t clahe: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::Mat disp_left;
	left_matcher->compute(main_grey, aux_grey, disp_left);
	disp_left.convertTo(output, CV_8UC1, 255.0 / (16 * m_param.max_disp), 0);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t SGBM: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;
#pragma endregion

#pragma region preprocess disparity map
	// disparity map preprocess
	cv::Mat img_blur;
	cv::Mat guide_rgb;
	if (real_max_disp > trigger_max_disp) {
		output = output(cv::Rect(
			m_param.max_disp * this->micro_dist_ratio,
			0,
			m_param.depth_size.width * this->micro_dist_ratio,
			m_param.depth_size.height * this->micro_dist_ratio)).clone();
		ShowImg("raw-depth", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 crop: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		// crop the surrounding zone with mis-calibration
		CropBall(output, 0.05); // 0.05
		ShowImg("crop-ball", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 crop-ball: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		// restrict the disparity value
		uint8_t max_disp_u8 = (uint8_t)std::fmin(
			real_max_disp * 1.2 * this->micro_dist_ratio * (256.0 / m_param.max_disp) - 1, 255);
		cv::threshold(output, output, max_disp_u8, 255, cv::THRESH_TOZERO_INV);
		ShowImg("restricted-depth", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 max clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		removeWhiteEdge(output);
		ShowImg("removeWhiteEdge", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 clean edges: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		removeSpot(output);
		ShowImg("removeSpot", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 remove spots: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		//// remove invalid disparity region
		//cv::threshold(output, output, 24, 255, cv::THRESH_TOZERO);
		//ShowImg("Remove Low Values", output);
		//tick_sub_stop = TICK();
		//IMAGE_DEPTH_LOGI("\t\t route#1 remove low values: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		//tick_sub_start = tick_sub_stop;

		HistBasedFilter(output, 0.04, true);
		ShowImg("HistBasedFilter1", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 min clip(zero ignored): %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		//cv::Mat guide_grey = main_grey(cv::Rect(m_param.max_disp * ratio, 0,
		//	m_param.depth_size.width * ratio, m_param.depth_size.height * ratio)).clone();
		//removeTextureless(output, guide_grey);
		//ShowImg("removeTextureless", output);
		//tick_sub_stop = TICK();
		//IMAGE_DEPTH_LOGI("\t\t route#1 remove textureless: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		//tick_sub_start = tick_sub_stop;

		guide_rgb = main_resized(cv::Rect(
			m_param.max_disp * this->micro_dist_ratio,
			0,
			m_param.depth_size.width * this->micro_dist_ratio,
			m_param.depth_size.height * this->micro_dist_ratio)).clone();
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 crop main: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		//cv::medianBlur(guide_rgb, img_blur, 5);
		//tick_sub_stop = TICK();
		//IMAGE_DEPTH_LOGI("\t\t route#1 blur main: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		//tick_sub_start = tick_sub_stop;

		//FillBlackHoles_iters(output, img_blur, 3);
		FillBlackHoles_new(output, guide_rgb, true);
		ShowImg("FillBlackHoles", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 fill holes: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		HistBasedFilter(output, 0.004, false);
		ShowImg("HistBasedFilter2", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 min clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		cv::resize(output, output, m_param.depth_size);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#1 upsample: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;
	}
	else {
		output = output(cv::Rect(m_param.max_disp, 0, m_param.depth_size.width,
			m_param.depth_size.height)).clone();
		ShowImg("raw-depth", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 crop: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		// chop the triangle corners on the right side: 90x90
		/*ChopCorners(output, 120);
		ShowImg("chop-corners", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 chop corners: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;*/

		// crop the surrounding zone with mis-calibration
		CropBall(output, 0.05); // 0.05
		ShowImg("crop-ball", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 crop-ball: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		// restrict the disparity value
		uint8_t max_disp_u8 = (uint8_t)std::fmin(
			real_max_disp * 1.2 * (256.0 / m_param.max_disp) - 1, 255);
		cv::threshold(output, output, max_disp_u8, 255, cv::THRESH_TOZERO_INV);
		ShowImg("restricted-depth", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 max clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		removeWhiteEdge(output);
		ShowImg("removeWhiteEdge", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 clean edges: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		removeSpot(output);
		ShowImg("removeSpot", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 remove spots: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		//// remove invalid disparity region
		//cv::threshold(output, output, 24, 255, cv::THRESH_TOZERO);
		//ShowImg("Remove Low Values", output);
		//tick_sub_stop = TICK();
		//IMAGE_DEPTH_LOGI("\t\t route#2 remove low values: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		//tick_sub_start = tick_sub_stop;

		HistBasedFilter(output, 0.04, true);
		ShowImg("HistBasedFilter1", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 min clip(zero ignored): %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		//HistBasedPeakFilter_(output, 0.004, 16);
		//ShowImg("HistBasedPeakFilter_", output);
		//tick_sub_stop = TICK();
		//IMAGE_DEPTH_LOGI("\t\t route#2 peak clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		//tick_sub_start = tick_sub_stop;

		//cv::Mat guide_grey = main_grey(cv::Rect(m_param.max_disp, 0, m_param.depth_size.width, m_param.depth_size.height)).clone();
		//removeTextureless(output, guide_grey);
		//ShowImg("removeTextureless", output);
		//tick_sub_stop = TICK();
		//IMAGE_DEPTH_LOGI("\t\t route#2 remove textureless: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		//tick_sub_start = tick_sub_stop;

		guide_rgb = main_(cv::Rect(m_param.max_disp, 0, m_param.depth_size.width, m_param.depth_size.height)).clone();
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 crop main: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		//cv::medianBlur(guide_rgb, img_blur, 5);
		//tick_sub_stop = TICK();
		//IMAGE_DEPTH_LOGI("\t\t route#2 blur main: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		//tick_sub_start = tick_sub_stop;

		//FillBlackHoles_iters(output, img_blur, 3);
		FillBlackHoles_new(output, guide_rgb, true);
		ShowImg("FillBlackHoles", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 fill holes: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		HistBasedFilter(output, 0.004, false);
		ShowImg("HistBasedFilter2", output);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t\t route#2 min clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;
	}

	cv::Mat img;
	if (real_max_disp > trigger_max_disp)
		img = main_(cv::Rect(m_param.max_disp, 0, m_param.depth_size.width, m_param.depth_size.height)).clone();
	else img = guide_rgb;
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t crop main/-: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	ChangeSaturation(img, 95);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t enhance saturation: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::Mat image_cc = img.clone();
	imageColorCorrection(image_cc, img);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t color correction: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::medianBlur(img, img, 5);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t blur main: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls =
		cv::ximgproc::createDisparityWLSFilterGeneric(false);
	wls->setSigmaColor(3);//3
	wls->setLambda(800);//800
	wls->filter(output, img, output);
	ShowImg("wls-small", output);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t\t wls: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;
#pragma endregion

	tick_stop = TICK();
	IMAGE_DEPTH_LOGI("\t ComputeDepthImpl DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
	return 0;
}


int tcl_depth_image::TCLStereoDepthAlg_cap::ComputeDepth(){
	IMAGE_DEPTH_LOGI("ComputeDepth BEGIN");

	double tick_freq = cv::getTickFrequency();
	double tick_start = TICK(), tick_stop;
	double tick_sub_start = tick_start, tick_sub_stop;

	cv::flip(im_L_small, im_L, -1);
    cv::flip(im_R_small, im_R, -1);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t flip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	if (ComputeDepthImpl(depth_u8, im_R, im_L)) {
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t ComputeDepthImpl: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		depth_out = depth_u8;

		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("ComputeDepth DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
		return 1;
	}
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t ComputeDepthImpl: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

    // transform back to original view
    cv::flip(depth_u8, depth_u8, -1);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t flip back: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	if (m_param.alg_type != TCL_IMAGE_DEPTH_ALG_FAST)
		cv::remap(depth_u8, depth_out, imap1, imap2, INTER_LINEAR);
	else depth_out = depth_u8;
	ShowImg("depth_out", depth_out);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t remap/-: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	if (m_param.dump_enabled) DumpImage(depth_out, m_param.dump_path, "depth_raw_", uid);

	tick_stop = TICK();
	IMAGE_DEPTH_LOGI("ComputeDepth DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));

	return 0;
};


void tcl_depth_image::TCLStereoDepthAlg_cap::Postprocess() {
	IMAGE_DEPTH_LOGI("Postprocess BEGIN");

	double tick_freq = cv::getTickFrequency();
	double tick_start = TICK(), tick_stop;
	double tick_sub_start = tick_start, tick_sub_stop;

	cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls =
		cv::ximgproc::createDisparityWLSFilterGeneric(false);

#pragma region resize_and_rotate
	cv::Mat im_R_rgb, disp_raw, out_;
	im_R_rgb = im_guide;
	if (m_param.orientation == TCL_IMAGE_ROTATE_NONE) {
		cv::resize(depth_out, disp_raw, target_size, cv::INTER_LINEAR);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_180) {
		cv::rotate(depth_out, disp_raw, cv::ROTATE_180);
		cv::resize(disp_raw, disp_raw, target_size, cv::INTER_LINEAR);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_COUNTER_CLOCKWISE_90) {
		cv::rotate(depth_out, disp_raw, cv::ROTATE_90_CLOCKWISE);
		cv::resize(disp_raw, disp_raw, target_size, cv::INTER_LINEAR);
	}
	else if (m_param.orientation == TCL_IMAGE_ROTATE_CLOCKWISE_90) {
		cv::rotate(depth_out, disp_raw, cv::ROTATE_90_COUNTERCLOCKWISE);
		cv::resize(disp_raw, disp_raw, target_size, cv::INTER_LINEAR);
	}
	ShowImg("rgb", im_R_rgb);
	if (m_param.dump_enabled) DumpImage(im_R_rgb, m_param.dump_path, "tcl_main", uid);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t resize main and rotate disp: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;
#pragma endregion

#pragma region depth_preprocess
	HistBasedFilter(disp_raw, 0.04, false);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t min-max clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	//#define CONVERT_DEPTH_MAP
#ifdef CONVERT_DEPTH_MAP
	// convert from disparity into depth map
	float disp_min_, disp_max_;
	disp_min_ = 1;
	disp_max_ = 255;
	Disparity2Depth(disp_raw, disp_min_, disp_max_);
	ShowImg("depth-raw", disp_raw);
	if (m_param.dump_enabled) DumpImage(disp_raw, m_param.dump_path, "depth_raw", uid);
#endif

#pragma endregion


#pragma region portrait_mask
	cv::Mat portrait_mask;
	if (m_param.alg_type == TCL_IMAGE_DEPTH_ALG_BEST) {
		cv::Mat nn_input;
		cv::Mat nn_output;
		cv::Size infer_size(512, 384);

		cv::resize(im_R_rgb, nn_input, infer_size);
		nn_output = cv::Mat::zeros(infer_size, CV_8UC1);

//#define USING_PYTHON_INTERFACE
#ifdef USING_PYTHON_INTERFACE
		cv::String nn_in_path, nn_out_path;
		nn_in_path = "nn_input.jpg";
		nn_out_path = "nn_output.png";

		cv::imwrite(nn_in_path, nn_input);
		ShowImg("portrait input", nn_input);

		char cmd_buf[256];
#ifdef _WIN32
		sprintf_s(cmd_buf, "python ../../../portrait/demo.py %s %s", nn_in_path.c_str(), nn_out_path.c_str());
#else
		sprintf(cmd_buf, "python ../../../portrait/demo.py %s %s", nn_in_path.c_str(), nn_out_path.c_str());
#endif
		system(cmd_buf);
		// read the output result from AI portrait model
		nn_output = cv::imread(nn_out_path, -1);
#else
		// call the C++ interface of AI model
		cv::Size size_req;
		IMAGE_DEPTH_LOGI("BEGIN init_segment");
		init_segment(PORTRAIT_MODEL_PATH, 0, 4);
		IMAGE_DEPTH_LOGI("END init_segment");
		get_inputsize(size_req.width, size_req.height,
			nn_input.size().width, nn_input.size().height);
		if (nn_input.size().width != size_req.width ||
				nn_input.size().height != size_req.height) {
				IMAGE_DEPTH_LOGE("Error in call AI portrait: incorrect input size!");
				exit(-1);
			}
		IMAGE_DEPTH_LOGI("BEGIN do_segment");
		do_segment(nn_input.data, nn_output.data,
			nn_input.size().width, nn_input.size().height);
		IMAGE_DEPTH_LOGI("END do_segment");
		release_segment();
#endif
		ShowImg("portrait mask", nn_output);
		if (m_param.dump_enabled) DumpImage(nn_output, m_param.dump_path, "portrait", uid);

		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t portrait mask: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

#define PORTRAIT_ONLY
#ifdef PORTRAIT_ONLY
		cv::Mat disp_raw_f32;
		// dicriminate portrait or non-portrait scene
		const uint8_t thres_portrait_val = 254;
		const float thres_portrait_ratio = 0.03;
		cv::Mat mask_conf, mat_res;
		cv::threshold(nn_output, mask_conf, thres_portrait_val, 255, cv::THRESH_BINARY);
		if(m_param.dump_enabled) DumpImage(mask_conf, m_param.dump_path, "conf", uid);
		cv::reduce(mask_conf, mat_res, 0, cv::REDUCE_SUM, CV_32FC1);
		cv::reduce(mat_res, mat_res, 1, cv::REDUCE_SUM, CV_32FC1);
		float portrait_ratio = mat_res.at<float>(0, 0) / (255.0 * infer_size.width * infer_size.height);
		IMAGE_DEPTH_LOGI("*********portrait_ratio= %.3f ***********", portrait_ratio);
		if(portrait_ratio > thres_portrait_ratio){
			IMAGE_DEPTH_LOGI("Portrait mode!!!");
			// merge the portrait mask with disparity map
			cv::resize(nn_output, portrait_mask, disp_raw.size());
			// smooth the portrait part
			//cv::Mat disp_portrait;
			//wls->setSigmaColor(3);//0.4
			//wls->setLambda(800);//8000
			//wls->filter(disp_raw, portrait_mask, disp_portrait);
			cv::Mat mask_fg_f32, disp_fg_f32;
			portrait_mask.convertTo(mask_fg_f32, CV_32FC1);
			//disp_portrait.convertTo(disp_fg_f32, CV_32FC1);
			disp_raw.convertTo(disp_fg_f32, CV_32FC1);
			mask_fg_f32 *= 1.0/255.0;
			disp_raw_f32 = disp_fg_f32.mul(mask_fg_f32);
			disp_raw_f32.convertTo(disp_raw, CV_8UC1);
			// replace zero-disparity pixels
			cv::Mat mask_zero = disp_raw == 0;
			cv::bitwise_and(32, mask_zero, mask_zero);
		    disp_raw += mask_zero;
		} else{
			IMAGE_DEPTH_LOGI("Non-portrait mode~");
		}
#else
		// merge the portrait mask with disparity map
		cv::resize(nn_output, portrait_mask, disp_raw.size());
		// smooth the foreground
		cv::Mat disp_portrait;
		wls->setSigmaColor(0.4);//0.4
		wls->setLambda(8000);//8000
		wls->filter(disp_raw, portrait_mask, disp_portrait);
		cv::Mat mask_fg_f32, disp_fg_f32;
		portrait_mask.convertTo(mask_fg_f32, CV_32FC1);
		disp_portrait.convertTo(disp_fg_f32, CV_32FC1);
		mask_fg_f32 *= 1.0/255.0;
		// smooth the background
		cv::Mat disp_background;
		wls->setSigmaColor(0.4);
		wls->setLambda(8000);
		wls->filter(disp_raw, disp_raw, disp_background);
		cv::Mat disp_bg_f32;
		disp_background.convertTo(disp_bg_f32, CV_32FC1);
		disp_raw_f32 = disp_fg_f32.mul(mask_fg_f32) + disp_bg_f32.mul(1 - mask_fg_f32);
		disp_raw_f32.convertTo(disp_raw, CV_8UC1);
#endif

		ShowImg("portrait_filtered", disp_raw);
	}

#pragma endregion


#define WITH_JBF
#ifdef WITH_JBF
	cv::Mat mask_jbf(disp_raw.size(), CV_8UC1, cv::Scalar(255));
	cv::Mat disp_jbf;

	cv::Mat im_R_rgb_enhanced;
	if (m_param.alg_type == TCL_IMAGE_DEPTH_ALG_FAST) {
		im_R_rgb_enhanced = im_R_rgb;
	}
	else {
		im_R_rgb_enhanced = im_R_rgb.clone();
		imageColorCorrection(im_R_rgb, im_R_rgb_enhanced);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t exposure correction: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;
	}

	ChangeSaturation(im_R_rgb_enhanced, 95);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t enhance saturation: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	jointBilateralFilter_(im_R_rgb_enhanced, disp_raw, mask_jbf, disp_jbf, 50, 15, 40, BORDER_DEFAULT);//80,15,40
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t jbf: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	cv::medianBlur(disp_jbf, disp_jbf, 3);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t median blur: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	ShowImg("jbf", disp_jbf);
	if (m_param.dump_enabled) DumpImage(disp_jbf, m_param.dump_path, "disp_jb", uid);

	wls->setSigmaColor(0.4);//0.4
	wls->setLambda(8000);//8000
	wls->filter(disp_jbf, disp_jbf, out_);
	ShowImg("wls", out_);
	tick_sub_stop = TICK();
	IMAGE_DEPTH_LOGI("\t wls: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
	tick_sub_start = tick_sub_stop;

	if (m_param.alg_type != TCL_IMAGE_DEPTH_ALG_FAST) {
		cv::Mat disp_jbf_f32, gray_norm;
		disp_jbf.convertTo(disp_jbf_f32, CV_32FC1);
		cv::normalize(disp_jbf_f32, gray_norm, 0.0, 255.0, cv::NORM_MINMAX);
		gray_norm = gray_norm / 255.0;
		cv::Mat out_f32;
		out_.convertTo(out_f32, CV_32FC1);
		out_f32 = out_f32.mul(gray_norm) + disp_jbf_f32.mul(1.0 - gray_norm);
		out_f32.convertTo(out_, CV_8UC1);
		ShowImg("merged", out_);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t merge: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;
	}
#endif


//#define WITH_FBS
#ifdef WITH_FBS
	// fast bilateral filter
	double fbs_spatial = 48.0; // 48
	double fbs_luma = 8.0; // 8
	double fbs_chroma = 8.0; // 8
	double fbs_depth = 32.0; // 32
	double fbs_lambda = 128.0; // 128
	int fbs_itr = 25;
	double fbs_err = 1E-5;
	int fbs_rate = 3;

	double start = TICK();
	double freq = cv::getTickFrequency();
	cv::Mat disp_fbs;
#ifdef WITH_JBF
	out_.copyTo(disp_raw);
#endif
	double stop = TICK();
	IMAGE_DEPTH_LOGI("HistBasedFilter: %.3f ms", 1000 * (stop - start) / freq);
	ShowImg("disp_raw", disp_raw);

	// get a binary mask of processing region using confidence map
	start = TICK();
	cv::Mat edge_mask, process_mask;
	int conf_thres = 255 - 32;
	// get laplacian edges
	cv::Mat conf_map;
	cv::Laplacian(disp_raw, conf_map, -1);
	conf_map = 255 - conf_map;
	ShowImg("conf_map", conf_map);
	edge_mask = conf_map < conf_thres;
	int dilate_radius = 24;
	cv::dilate(edge_mask, process_mask, cv::Mat(),
		cv::Point(-1, -1), dilate_radius);
	stop = TICK();
	IMAGE_DEPTH_LOGI("dilate: %.3f ms", 1000 * (stop - start) / freq);
	ShowImg("edge_mask", edge_mask);
	ShowImg("process_mask", process_mask);

	start = TICK();
	cv::Ptr<FastBilateralSolverFilter_> fbs =
		cv::Ptr<FastBilateralSolverFilter_>(
			FastBilateralSolverFilter_::create(
				im_R_rgb,
				disp_raw,
				fbs_spatial,
				fbs_luma,
				fbs_chroma,
				fbs_depth,
				fbs_lambda,
				fbs_itr,
				fbs_err,
				process_mask,
				fbs_rate,
				edge_mask));
	fbs->filter(disp_raw, conf_map / 255.0, disp_fbs);
	stop = TICK();
	IMAGE_DEPTH_LOGI("FastBilateralSolverFilter: %.3f ms",
		1000 * (stop - start) / freq);
	ShowImg("depth_fbs", disp_fbs);

	// fill unprocessed points
	start = TICK();
	cv::Mat tmp(disp_fbs.size(), CV_8UC1, cv::Scalar(0));
	FillBlackHoles_new(disp_fbs, im_R_rgb, true);
	stop = TICK();
	IMAGE_DEPTH_LOGI("Fill: %.3f ms", 1000 * (stop - start) / freq);
	ShowImg("depth_fbs_filled", disp_fbs);

	// check bad foreground expansion
	start = TICK();
	//cv::max(disp_fbs, disp_raw, disp_raw);
	disp_fbs.copyTo(disp_raw);
	stop = TICK();
	IMAGE_DEPTH_LOGI("erase foreground expansion: %.3f ms", 1000 * (stop - start) / freq);
	ShowImg("depth_fbs_blur", disp_raw);

	// use the filtered gray to guide
	start = TICK();
#ifndef WITH_JBF
	cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls =
		cv::ximgproc::createDisparityWLSFilterGeneric(false);
#endif
	wls->setSigmaColor(3);
	wls->setLambda(8000);
	wls->filter(disp_raw, disp_raw, out_);
	stop = TICK();
	IMAGE_DEPTH_LOGI("WLS Filter: %.3f ms", 1000 * (stop - start) / freq);
	ShowImg("depth_fbs_wls", out_);
#endif


#ifdef CONVERT_DEPTH_MAP
	// convert back into disparity
	Depth2Disparity(out_, disp_min_, disp_max_);
#endif

#define SHRINK_EDGE
#ifdef SHRINK_EDGE
	cv::Mat out_1;
	if (m_param.alg_type != TCL_IMAGE_DEPTH_ALG_FAST) {
		Erode_v3_fastest(out_1, out_, im_R_rgb, 2, 8, 255);
		Erode_v3_fastest(out_, out_1, im_R_rgb, 2, 8, 255);
		Dilate_v3_fastest(out_1, out_, im_R_rgb, 2, 8, 8);
		Dilate_v3_fastest(out_, out_1, im_R_rgb, 2, 8, 8);
		Dilate_v3_fastest(out_1, out_, im_R_rgb, 1, 8, 4);
		Dilate_v3_fastest(out_, out_1, im_R_rgb, 1, 8, 4);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t shrink edge: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;
	}
#endif

#pragma region disparity_refinement
	if (m_param.alg_type != TCL_IMAGE_DEPTH_ALG_FAST) {
		HistBasedFilter(out_, 0.04, false);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t 2nd min-max clip: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;

		cv::medianBlur(out_, out_, 3);
		tick_sub_stop = TICK();
		IMAGE_DEPTH_LOGI("\t 2nd median blur: %.1f ms", (1000 * (tick_sub_stop - tick_sub_start) / tick_freq));
		tick_sub_start = tick_sub_stop;
	}
#pragma endregion

	ShowImg("final-disp", out_);
	out_.copyTo(depth_out);
	WaitKey(0);

	// debug: check if the AI portrait model performs correctly
	//cv::resize(portrait_mask, depth_out, out_.size());

	tick_stop = TICK();
	IMAGE_DEPTH_LOGI("Postprocess DONE: %.1f ms", (1000 * (tick_stop - tick_start) / tick_freq));
}


void tcl_depth_image::TCLStereoDepthAlg_cap::Compute(
	cv::Mat & depth,
	const cv::Mat & im_aux, // gray scale
	const cv::Mat & im_main, // rgb
	const cv::String & uid,
	float fx, // in ratio form, like (0.5, 0.5) being the center of the image
	float fy
	){
	this->uid = uid;
	this->fx = fx;
	this->fy = fy;
    double tick_start, tick_stop;
    double tick_freq = cv::getTickFrequency();
    tick_start = TICK();

	if (!init_success) {
		IMAGE_DEPTH_LOGE("Not available: module uninitialized or initialization failed!");
		depth = cv::Mat::zeros(target_size, CV_8UC1) + 255;
		return;
	}

	this->Preprocess(im_aux, im_main);
    tick_stop = TICK();
	IMAGE_DEPTH_LOGI("DEPTH PREPROCESS TIME: %6.3f ms", 1000*(tick_stop - tick_start) / tick_freq);
    tick_start = TICK();

	if (this->ComputeDepth()) {
		depth_out.copyTo(depth);
		tick_stop = TICK();
		IMAGE_DEPTH_LOGI("DEPTH COMPUTATION TIME: %6.3f ms", 1000 * (tick_stop - tick_start) / tick_freq);
		tick_start = TICK();
		return;
	};
    tick_stop = TICK();
	IMAGE_DEPTH_LOGI("DEPTH COMPUTATION TIME: %6.3f ms", 1000*(tick_stop - tick_start) / tick_freq);
    tick_start = TICK();

	this->Postprocess();
    tick_stop = TICK();
	IMAGE_DEPTH_LOGI("DEPTH POSTPROCESS TIME: %6.3f ms", 1000*(tick_stop - tick_start) / tick_freq);
    depth_out.copyTo(depth);
}


float tcl_depth_image::TCLStereoDepthAlg_cap::GetFocusDistance(
        float focus_x,
        float focus_y) {
    if (focus_x<0 || focus_x>=1 || focus_y<0 || focus_y>=1){
		return -1;
    }
    Mat point_(4, 1, CV_64F);
    float x_, y_, d_;
    int radius = 3; // the radius to search the depth of focus
    float sum_of_depth;
    int begin_x, begin_y, end_x, end_y;

    x_ = (1 - focus_x) * depth_out.size().width;
    y_ = (1 - focus_y) * depth_out.size().height;
    begin_x = max(0, int(x_) - radius);
    begin_y = max(0, int(y_) - radius);
    end_x = min(depth_out.size().width, int(x_) + radius);
    end_y = min(depth_out.size().height, int(y_) + radius);
    sum_of_depth = 0.f;

    for (int i = begin_y; i<end_y; ++i) {
        for (int j = begin_x; j<end_x; ++j) {
            sum_of_depth += depth_out.at<unsigned char>(i, j);
        }
    }

    d_ = sum_of_depth / ((end_x - begin_x) * (end_x - begin_x));
    d_ = d_ * 16 * m_param.max_disp / 255;
    point_.at<double>(0, 0) = x_;
    point_.at<double>(1, 0) = y_;
    point_.at<double>(2, 0) = d_;
    point_.at<double>(3, 0) = 1.0F;
    Mat physic_point = Q * point_;
    float dis = physic_point.at<double>(0, 2) / physic_point.at<double>(0, 3);
    return dis * 16;
}
